/******************************************
 *
 *
 *****************************************/

#include "stdint.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "cmsis_os.h"
#include "init.h"
#include "can.h"
#include "communicate.h"
#include "usart.h"
#include "bsp_app.h"
#include "bsp_led.h"
#include "bsp_can.h"
#include "bsp_uart.h"
#include "imu_task.h"


#include "chassis_protocol.h"
#include "SEGGER_RTT.h"





extern osThreadId communicate_task_t;
extern QueueHandle_t xQueueCan;
extern QueueHandle_t xQueueDbus;

joySpeed_t joy_cmd_vel;
chassis_detal_speed_t chassis_detal_speed_send;
chassis_raw_speed_t chassis_raw_speed_send;
chassis_raw_speed_t chassis_raw_speed_recv;
rosCtlChassisSpeed_t rosCtlSpeed;  // globle variable control
char print_buf[300];
enum {CHASSIS_STOP,jOY_CTL,ROS_CRL};

void user_dbus_task(void const * argument)
{
    joystick_t joy_cmd;
    BaseType_t dbusResult;
//  portTickType xLastWakeTime;
//  const portTickType xFrequency = 100;
    const TickType_t xMaxBlockTime = pdMS_TO_TICKS(20);

    while(1)
    {
        //xLastWakeTime = xTaskGetTickCount();
        dbusResult = xQueueReceive(xQueueDbus,(void *)&joy_cmd,(TickType_t)xMaxBlockTime);

        if(pdPASS == dbusResult)
        {
//      SEGGER_RTT_printf(0,"speed: %d  turn: %d SW1: %d  SW2: %d ",
//                        joy_cmd.speed,joy_cmd.turn,joy_cmd.sw1, joy_cmd.sw2);
            switch(joy_cmd.sw1)
            {
            case 1:
                joy_cmd_vel.is_enable = CHASSIS_STOP;
                break;

            case 2:
                joy_cmd_vel.is_enable = ROS_CRL;
                break;

            case 3:
                joy_cmd_vel.is_enable = jOY_CTL;
                break;

            default:
                joy_cmd_vel.is_enable = CHASSIS_STOP;
                break;
            }

            switch(joy_cmd.sw2)
            {
            case 1:
                joy_cmd_vel.max_speed = SPEED_LOW;
                break;

            case 2:
                joy_cmd_vel.max_speed = SPEED_HIGHT;
                break;

            case 3:
                joy_cmd_vel.max_speed = SPEED_MID;
                break;

            default:
                joy_cmd_vel.max_speed = 0;
                break;
            }

            //enable
            joy_cmd_vel.linear_velocity = joy_cmd.speed/MAX_JOYSTICK_VALUE*joy_cmd_vel.max_speed;
            joy_cmd_vel.angle_velocity = joy_cmd.turn/MAX_JOYSTICK_VALUE*joy_cmd_vel.max_speed/2;

            if(joy_cmd_vel.angle_velocity > 0.7f)
            {
                joy_cmd_vel.angle_velocity = 0.7f;
            }
        }
        else
        {
            joy_cmd_vel.linear_velocity = 0;
            joy_cmd_vel.angle_velocity = 0;
        }

        sprintf(print_buf, "               actl: %3.3lf\n",joy_cmd_vel.angle_velocity);
        SEGGER_RTT_printf(0,print_buf);
        //DelayUntil(&xLastWakeTime, xFrequency);
        vTaskDelay(10);
    }
}

/********************************
*@brief : send chassis control can msg
*
***********************************/
void timerChassisCtlCallback(TimerHandle_t xTimer)
{
    //static TickType_t xTimeNow;
    float linear_velocity = 0;
    float angle_velocity = 0;
    chassis_raw_speed_t chassis_speed_raw;
    LED_8_Toggle();

    switch(joy_cmd_vel.is_enable)
    {
    case CHASSIS_STOP:
        linear_velocity = 0;
        angle_velocity = 0;
        break;

    case jOY_CTL:
        linear_velocity  = joy_cmd_vel.linear_velocity;
        angle_velocity = joy_cmd_vel.angle_velocity;
        break;

    case ROS_CRL:
        linear_velocity = rosCtlSpeed.linear_velocity;
        angle_velocity = rosCtlSpeed.angle_velocity;
        break;

    default:
        linear_velocity = 0;
        angle_velocity = 0;
        break;
    }

    chassis_speed_raw = calc_raw_speed(linear_velocity,angle_velocity);
//  SEGGER_RTT_printf(0," speed_l: %d  %d \n speed_r: %d  %d \n",\
//                    chassis_speed_raw.front_left,chassis_speed_raw.front_right,\
//                    chassis_speed_raw.rear_left,chassis_speed_raw.rear_right);
    send_front_left_speed(chassis_speed_raw);
    send_front_right_speed(chassis_speed_raw);
    vTaskDelay(2);
    send_rear_left_speed(chassis_speed_raw);
    send_rear_right_speed(chassis_speed_raw);
    //set 0 linear angle
    rosCtlSpeed.angle_velocity = 0;
    rosCtlSpeed.linear_velocity = 0;
    joy_cmd_vel.linear_velocity = 0;
    joy_cmd_vel.angle_velocity = 0;
}





/******************************
* report chassis speed can msg
*
********************************/
void timerChassisReportCallback(TimerHandle_t xTimer)
{
    userCanBuf_t can_buff;
    static int front_left = 0,front_right = 0;
    static int front_left_pre = 0,front_right_pre = 0;
    double linear_velocity ,angular_velocity;
    double  angular_velocity_1;
    short linear_tmp ,angular_tmp;
    int front_left_delta,front_right_delta;
    // Calculator
    front_left = chassis_raw_speed_recv.front_left;
    front_right = chassis_raw_speed_recv.front_right;
    front_left_delta = -(front_left - front_left_pre);//测试可得左轮反馈速度位负
    front_right_delta = (front_right - front_right_pre);
    // 左右轮速度和除以时间
//  linear_velocity = (double)(front_left_delta+front_right_delta)/2/VEL_PARAM/0.04;
    linear_velocity = (double)(front_left_delta+front_right_delta)/DELTA_TO_VEL;  //VEL_PARAM/0.008;

    if(linear_velocity > MAX_LIN_VELOCITY)
    {
        linear_velocity = MAX_LIN_VELOCITY;
    }

    linear_tmp = (short)(linear_velocity *1000); // 线速度
    // 从imu获取角速度
    // 右手坐标系逆时针为正
    //angular_velocity = -mpu_sensor.wz;
    angular_velocity = (double)(front_left_delta - front_right_delta)/DELTA_TO_VEL/WHEEL_BASE;  //VEL_PARAM/0.008;
    angular_tmp = (short)(angular_velocity*1000);
    //
    sprintf(print_buf, " am: %3.3lf ac: %3.3lf\n",\
            angular_velocity,angular_velocity_1);
    SEGGER_RTT_printf(0,print_buf);
    chassis_status_package(&can_buff,linear_tmp,angular_tmp);
    // 发送线速度角度
    chasis_repeort_vel(&can_buff);
    front_left_pre = front_left;
    front_right_pre = front_right;
}

void chassis_task(void const *argument)
{
//  uint8_t app;
//  app = get_sys_cfg();
    BaseType_t xResult;
    userCanBuf_t userRecvBuf;
//  portTickType xLastWakeTime;
//  const portTickType xFrequency = 100;
    /* 获取系统时间 */
//  xLastWakeTime = xTaskGetTickCount();
//  const TickType_t xMaxBlockTime = pdMS_TO_TICKS(20);
    const TickType_t xMaxBlockTime = pdMS_TO_TICKS(200);

    while(1)
    {
//      osDelay(1);
//      CAN_Send_Msg(&hcan2, userSendBuf.can_data, userSendBuf.ID+1, 8);
        xResult = xQueueReceive(xQueueCan,(void *)&userRecvBuf,(TickType_t)xMaxBlockTime);

        if(xResult == pdPASS)
        {
            switch(userRecvBuf.port)
            {
            case CAN1_PORT:

                // ros control msg
                if(CAN_ID_MOTION_CONTROL == userRecvBuf.ID)
                {
                    rosCtlSpeed = ros_decode_msg(&userRecvBuf);
                }

                break;

            case CAN2_PORT:
                chassis_raw_speed_recv = chassis_decode_msg(&userRecvBuf);
                break;
            }
        }

        vTaskDelay(5);
    }
}





/*********** END OF FILE *************/

